#include "multi_process_points.h"
namespace perception
{
    namespace tool
    {
        template <typename T1, typename T2, typename T3>
        void MultiProcessPoints::SplitPoints(const typename pcl::PointCloud<T1>::Ptr &data_in, std::vector<typename pcl::PointCloud<T2>::Ptr> &data_out, T3 &config)
        {
            if (data_in->points.empty())
            {
                std::cout << "no lidar points input\n";
                return;
            }
            // 找到第一个点云为空的点云数据id
            int start_num = 0;
            for (; start_num < data_out.size(); start_num++)
            {
                if (data_out[start_num]->points.empty())
                    break;
            }
            if (start_num == 16)
                start_num = 0;
            // 输入点云转换为输出点云格式
            typename pcl::PointCloud<T2>::Ptr cloud(new pcl::PointCloud<T2>);
            pcl::copyPointCloud(*data_in, *cloud);
            int num_ = cloud->points.size() / config.max_thread_num; // 通过分成num份点云,得到每一分点云数;
            // 1.如果小于每份最小点云数
            if (num_ < config.one_points_min_num)
            {
                int min_num = cloud->points.size() / config.one_points_min_num;
                if (min_num < 2)
                {
                    data_out[start_num].reset(new pcl::PointCloud<T2>);
                    data_out[start_num] = cloud;
                    return;
                }
                for (int i = 0; i < min_num; i++)
                {
                    if (i == min_num - 1)
                    {
                        data_out[start_num].reset(new pcl::PointCloud<T2>);
                        data_out[start_num]->points.assign(cloud->points.begin() + i * config.one_points_min_num, cloud->points.end());
                        break;
                    }
                    data_out[start_num].reset(new pcl::PointCloud<T2>);
                    data_out[start_num]->points.assign(cloud->points.begin() + i * config.one_points_min_num, cloud->points.begin() + (i + 1) * config.one_points_min_num);
                    start_num++;
                }
                return;
            }
            // 2.大于最大点云数
            if (num_ > config.one_points_max_num)
            {
                int max_num = cloud->points.size() / config.one_points_max_num;
                for (int i = 0; i < max_num; i++)
                {
                    if (i == max_num - 1)
                    {
                        data_out[start_num].reset(new pcl::PointCloud<T2>);
                        data_out[start_num]->points.assign(cloud->points.begin() + i * config.one_points_max_num, cloud->points.end());
                        start_num++;
                        break;
                    }
                    data_out[start_num].reset(new pcl::PointCloud<T2>);
                    data_out[start_num]->points.assign(cloud->points.begin() + i * config.one_points_max_num, cloud->points.begin() + (i + 1) * config.one_points_max_num);
                    start_num++;
                }
                return;
            }
            // 3.正常
            for (int i = 0; i < config.max_thread_num; i++)
            {
                data_out[start_num].reset(new pcl::PointCloud<T2>);
                data_out[start_num]->points.assign(cloud->points.begin() + i * num_, cloud->points.begin() + (i + 1) * num_);
                start_num++;
            }
        }
        template <typename T1, typename T2>
        void MultiProcessPoints::MergePoints(std::vector<typename pcl::PointCloud<T1>::Ptr> &data_in, typename pcl::PointCloud<T2>::Ptr &data_out)
        {
            if (data_in.empty())
                return;
            for (auto &p : data_in)
            {
                data_out->points.insert(data_out->points.end(), p->points.begin(), p->points.end());
            }
        }
    }
}